// Copyright 2022, Uglyhack

use crate::Car;
use rand::Rng;
use std::ffi::OsString;

#[derive(Debug, Clone)]
enum Step {
    SteerAngle { time: f64, angle: f64 },
    SetAngle { time: f64, angle: f64 },
    Reverse { time: f64, reverse: bool },
}

impl Step {
    fn time(&self) -> f64 {
        match self {
            Step::SteerAngle { time, angle: _ } => *time,
            Step::SetAngle { time, angle: _ } => *time,
            Step::Reverse { time, reverse: _ } => *time,
        }
    }

    fn set_time(&mut self, set_time: f64) {
        match self {
            Step::SteerAngle { time, angle: _ } => *time = set_time,
            Step::SetAngle { time, angle: _ } => *time = set_time,
            Step::Reverse { time, reverse: _ } => *time = set_time,
        }
    }

    fn mutate<T: Rng>(&mut self, rnd: &mut T, variation: f64) {
        match self {
            Step::SteerAngle { time, angle } => {
                *time += (rnd.gen::<f64>() - 0.5) * 0.1 * variation;
                *time = time.max(0.00001);
                *angle += (rnd.gen::<f64>() - 0.5) * 0.01 * variation;
                *angle = angle.clamp(-Car::MAX_ANGLE, Car::MAX_ANGLE);
            }
            Step::SetAngle { time: _, angle: _ } => {}
            Step::Reverse { time, reverse: _ } => {
                if *time != 0. {
                    *time += (rnd.gen::<f64>() - 0.5) * 0.1 * variation;
                    *time = time.max(0.00001);
                }
            }
        }
    }

    fn apply(&self, car: &mut Car) {
        match self {
            Step::SteerAngle { time: _, angle } => {
                car.w = *angle;
            }
            Step::SetAngle { time: _, angle } => {
                car.a = *angle;
            }
            Step::Reverse { time: _, reverse } => {
                car.reverse = *reverse;
            }
        }
    }

    fn print(&self) {
        match self {
            Step::SteerAngle { time, angle } => {
                println!("  {} {}", time, angle)
            }
            Step::SetAngle { time, angle } => {
                println!("  {} angle {}", time, angle)
            }
            Step::Reverse {
                time,
                reverse: true,
            } => {
                println!("  {} reverse", time)
            }
            Step::Reverse {
                time,
                reverse: false,
            } => {
                println!("  {} forward", time)
            }
        }
    }
}

#[derive(Clone, Debug)]
pub struct Instructions {
    steps: Vec<Step>,
    setup: Vec<Step>,
    pub variation: f64, // How much to vary while optimizing
}

impl Instructions {
    /// Create empty instructions file
    pub fn new() -> Self {
        Self {
            steps: Vec::new(),
            setup: Vec::new(),
            variation: 1.0,
        }
    }
    /// Read instructions from route.txt
    pub fn read_from_file(filename: &OsString) -> Self {
        let mut retval = Self::new();

        for line in std::fs::read_to_string(filename).unwrap().lines() {
            let mut parts = line.split(' ');
            let time = parts.next().unwrap().parse().unwrap();
            let nextstep = match parts.next() {
                None => {
                    panic!("Malformed instructions file");
                }
                Some("angle") => {
                    let angle = parts.next().unwrap().parse().unwrap();
                    Step::SetAngle { time, angle }
                }
                Some("reverse") => Step::Reverse {
                    time,
                    reverse: true,
                },
                Some("forward") => Step::Reverse {
                    time,
                    reverse: false,
                },
                number => {
                    let angle = number.unwrap().parse().unwrap();
                    Step::SteerAngle { time, angle }
                }
            };
            if time == 0. {
                retval.setup.push(nextstep);
            } else {
                retval.steps.push(nextstep);
            }
        }

        retval
    }

    /// Whether there are any actual steps in these instructions
    pub fn has_steps(&self) -> bool {
        self.steps.len() != 0
    }

    pub fn mutate(&mut self) {
        let mut rnd = rand::thread_rng();
        for s in self.steps.iter_mut() {
            s.mutate(&mut rnd, self.variation);
        }

        self.steps
            .sort_unstable_by(|a, b| a.time().partial_cmp(&b.time()).unwrap());
    }

    pub fn print_all(&self) {
        for s in &self.setup {
            s.print();
        }
        for s in &self.steps {
            s.print();
        }
    }

    pub fn apply(&self, car: &mut Car, time: f64) {
        for s in &self.steps {
            if time > s.time() {
                s.apply(car);
            }
        }
    }

    pub fn setup(&self, car: &mut Car) {
        for s in &self.setup {
            s.apply(car);
        }
    }

    /// Reduce instances where the car goes back and forth along the same path
    pub fn optimize_reverse(&mut self) {
        // First we find the indexes of all the reversing steps
        let reversals: Vec<usize> = self
            .steps
            .iter()
            .enumerate()
            .filter_map(|s| {
                if let (i, Step::Reverse { .. }) = s {
                    Some(i)
                } else {
                    None
                }
            })
            .collect();

        for i in reversals {
            // Make sure there are enough items in the list to do our trick
            if i != 0 && (i + 1) != self.steps.len() {
                // Get the mutable references we need
                let (_, to_edit) = self.steps.split_at_mut(i - 1);
                let mut to_edit_iter = to_edit.iter_mut();

                let base = to_edit_iter.next().unwrap();
                let reversal = to_edit_iter.next().unwrap();
                let redirect = to_edit_iter.next().unwrap();

                // Now figure out how much we can shift it
                // We can shift at most by base -> reversal, and also at most by reversal->redirect
                // Multiply by .999 to leave a bit of room for sorting
                let timeshift =
                    (redirect.time() - reversal.time()).min(reversal.time() - base.time()) * 0.999;

                // Apply the shift
                reversal.set_time(reversal.time() - timeshift);
                redirect.set_time(redirect.time() - 2. * timeshift);
                for s in to_edit_iter {
                    s.set_time(s.time() - 2. * timeshift);
                }
            }
        }
    }
}
